#include <Wire.h>
#include <JY901_Mega2560.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Stream.h>

#define LEFT_BUTTON_PIN   2
#define RIGHT_BUTTON_PIN  3
#define ENABLE485_PIN     8

#define LEFT_MOVE   -1
#define RIGHT_MOVE  1
#define STOP_MOVE   0

//软件追踪时判断移动的最小度数（差值）
#define MIN_STOP_DEGREE   0.9

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
 
void setup()  
{                
  Serial.begin(9600);//PC
  Serial1.begin(9600);//JY-GPSIMU
  Serial2.begin(2400);//PTS-326
  Serial3.begin(9600);//Bluetooth
//  BT.setTimeout(100);
  //OLED启动
  delay(500);
  // by default, we'll generate the high voltage from the 3.3v line internally! (neat!)
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  display.setTextColor(WHITE);  //设置字体颜色白色
  Serial.println(F("SSD1306 allocation"));
  
  //控制按钮启动
  pinMode(LEFT_BUTTON_PIN, INPUT_PULLUP);
  pinMode(RIGHT_BUTTON_PIN, INPUT_PULLUP);
  Serial.println(F("control button start"));
  
  //rs485启动
  pinMode(ENABLE485_PIN, OUTPUT);
  delay(10);
  digitalWrite(ENABLE485_PIN, HIGH);
  Serial.println(F("RS485 control start"));
  
}
bool hold_button = false;//按键按住
bool moving_to_aim = false;//接收指令移动到目标
bool moving = false;//旋转器正在移动

float AZ_now;
float EL_now; 

float AZ_aim;
float EL_aim;

String command = "";
 
void loop() 
{
  float AZ_delta;
  float EL_delta;
  
  float temp_AZ;
  //显示输入数据

  while (Serial1.available()) 
  {
    JY901.CopeSerialData(Serial1.read()); //Call JY901 data cope function
  }
  temp_AZ = ((float)JY901.stcAngle.Angle[2]/32768*180);
  if(temp_AZ < 0)
  {
    AZ_now = -temp_AZ;
  }
  else
  {
    AZ_now = 360 - temp_AZ;
  }
  EL_now = (float)JY901.stcAngle.Angle[0]/32768*180;

  
   while (Serial.available() > 0)
  {
    char c = (char)Serial.read();
    if (c == '\n'||c == '\r')  //the enter or return ascii
      {
        process_command(command,&Serial);
        command = "";
      }
    else
       command += c;
       
  }

     while (Serial3.available() > 0)
  {
    char c = (char)Serial3.read();
    if (c == '\n'||c == '\r')  //the enter or return ascii
      {
        process_command(command,&Serial3);
        command = "";
      }
    else
       command += c;
       
  }

  AZ_delta = AZ_aim - AZ_now;
  EL_delta = EL_aim - EL_now;
  
  
  display.clearDisplay();
  display.setCursor(0,0); //设置字体的起始位置
  
  display.setTextSize(1);   //设置字体大小
  display.print("AZ:");display.println(AZ_now);display.print(AZ_aim);display.print("  ");display.println(AZ_delta);
  display.print("EL:");display.println(EL_now);display.print(EL_aim);display.print("  ");display.println(EL_delta);
  display.setTextSize(1);   //设置字体大小
  display.print("BL:");display.println((float)JY901.stcAngle.Angle[1]/32768*180);
  //display.print("GPS info\n20");
  display.print("20");  display.print(JY901.stcTime.ucYear);display.print("-");display.print(JY901.stcTime.ucMonth);display.print("-");display.print(JY901.stcTime.ucDay);
  display.print(" ");display.print(JY901.stcTime.ucHour);display.print(":");display.print(JY901.stcTime.ucMinute);display.print(":");display.println((float)JY901.stcTime.ucSecond+(float)JY901.stcTime.usMiliSecond/1000);
  display.print("Lo:");display.print(JY901.stcLonLat.lLon/10000000);display.print(".");display.print((int)(JY901.stcLonLat.lLon % 10000000)/60);
  display.print(" La:");display.print(JY901.stcLonLat.lLat/10000000);display.print(".");display.print((int)(JY901.stcLonLat.lLat % 10000000)/60);
  display.print(" He:");display.println((float)JY901.stcGPSV.sGPSHeight/10);
  display.display();  //显示信息
  
  //响应按钮移动
  if(digitalRead(LEFT_BUTTON_PIN) == LOW)
  {    
    if(!hold_button)
    {
//      Serial.println(F("Push left button"));
      rotor_move(LEFT_MOVE);
      moving = true;
      hold_button = true;
    }
    return;
  }
  
  if(digitalRead(RIGHT_BUTTON_PIN) == LOW)
  {
    if(!hold_button){
//      Serial.println(F("Push right button"));
      rotor_move(RIGHT_MOVE);
      moving = true;
      hold_button = true;
      }
      return;
  }
  //释放按钮  
  if(hold_button)
  {
    rotor_move(STOP_MOVE);
    hold_button = false;
    //中断软件控制移动
    moving_to_aim = false;
    moving = false;
    return;
  }

  //Easycomm收到移动指令
  if(moving_to_aim)
  {
    //绕线，去掉
//    if(AZ_delta>180)
//    {
//      AZ_delta -= 360;
//    }
//    if(AZ_delta< -180)
//    {
//      AZ_delta += 360;
//    }
    if(AZ_delta <= MIN_STOP_DEGREE && AZ_delta >= -MIN_STOP_DEGREE)
    {
      rotor_move(STOP_MOVE);
      moving_to_aim = false;
      return;
    }
    if(AZ_delta > 0 && !moving)
    {
      rotor_move(RIGHT_MOVE);
    }
    else
    {
      rotor_move(LEFT_MOVE);
    }
    
  }
  


  
  
} 

void process_command(String str_command,Stream * myserial)
{

  if (str_command.substring(0,5) == "AZ EL")
  {
//      Serial.sprintf("AZ%.1f EL%.1f",AZ_now,EL_now);
    myserial->print("AZ");
    myserial->print(AZ_now,1);
    myserial->print(" EL");
    myserial->print(EL_now,1);
    myserial->print("\r");
  }
  else if(str_command.startsWith("AZ") && str_command[2] != 0x20 )
  {

    int index_1st_space = str_command.indexOf(" ");
    if (index_1st_space != -1)
    {    

      AZ_aim = str_command.substring(2,index_1st_space).toFloat();
      moving_to_aim = true;
      int index_EL = str_command.indexOf("EL",index_1st_space);
      int index_2nd_space = str_command.indexOf(" ",index_EL);
      if (index_EL != -1)
      {
        EL_aim =  str_command.substring(index_EL+2,index_2nd_space).toFloat();
        
      }
//      display.println(str_command);
//      display.println(index_EL);
//      display.println(index_2nd_space);
//      display.print(AZ_aim,1);
//      display.print("EL");
//      display.println(EL_aim,1);
//      display.display();
//      delay(2000);
    
    }
  }
  else if (str_command == "SA SE ")
  {
    rotor_move(STOP_MOVE);
    moving_to_aim = false;
  }
  else if (str_command == "")
  {
  return;  
  }
  else
  {
  display.clearDisplay();
  display.setCursor(0,0); //设置字体的起始位置
  display.print("Unknow:");
  display.println(str_command);
  for(int i=0;i<str_command.length();i++)
  {
  display.print(String(str_command[i],HEX));
  }
  display.display();
  delay(10000);
  }
}


int rotor_move(int command)
{
  //旋转器使用的PELCO_D协议，其中速度只能为0x30
    switch (command)
    {
      case LEFT_MOVE:
        Serial2.write("\xFF\x01\x00\x04\x30\x30\x65",7);      
        break;
      case RIGHT_MOVE:
        Serial2.write("\xFF\x01\x00\x02\x30\x30\x63",7);
        break;
      case STOP_MOVE:
        Serial2.write("\xFF\x01\x00\x00\x00\x00\x01",7);
        break;  
      default:
        // statements
        break;
    }
  
}
